
<html>

<head>
<title>HostMot2 Encoder</title>
</head>


<body>
<center>

<table width=1000 cellpadding=10 border=1>

    <tr>
        <td>

            <p>Velocity estimation is made possible by a cool feature
            of the HostMot2 firmware:

            <p>The FPGA synthesizes a configurable-frequency "timestamp
            clock" by dividing ClockLow by the value in the Quadrature
            Counter Timestamp Divider Register.  ClockLow is 33 MHz on
            the PCI cards and 50 MHz on the 7i43.

            <p>The current value of the Timestamp Clock can be read from
            the 16-bit Timestamp Count (TSC) register.

            <p>When a quadrature counter instance in the HostMot2 FPGA
            detects a transition in its input Gray code, it increments
            the count and latches both the (16-bit) count and the 16
            bits of the timestamp clock into the Counter Register.

        </td>
    </tr>

    <tr>
        <td colspan=2>
            <p>Some Random Notes:
            <ul>

                <li><p>The encoder Count & Time ("C&T") are both 16-bit
                registers, which are latched and read together.

                <li><p>The encoder Count is initialized to 0 at firmware
                load time.

                <li><p>The Timestamp clock runs at 1 MHz (can run
                faster, but 1 meg is pretty fast and makes it easy to
                think about).

                <li><p>The Timestamp Counter is 16 bits wide.  It rolls
                over about every 65 milliseconds.

                <li><p>In the timing pics below i'm using a very
                slow servo loop of about 50 Hz.  The interval between
                reading the C&T register and the TSC register is also
                rediculously large.

            </ul>
        </td>
    </tr>
</table>


<br>
<hr>
<br>


<table width=1000 cellpadding=10 border=1>

    <tr>
        <td>

            <p>The velocity estimator used by the driver is similar
            to one described by David Auslander in a paper titled "<a
            href="http://repositories.cdlib.org/its/path/reports/UCB-ITS-PRR-95-3/">
            Vehicle-based Control Computer Systems</a>" (UCB ITS PRR
            95 3).

        </td>
    </tr>

    <tr>
        <td align=left valign=middle>
            <p>Algorithm notes:

            <ol>

                <li>

                    <p>The algorithm maintains some internal state.

                    <ul>

                        <li>

                            <p>Current-Motion-Mode: Stopped or Moving.

                            <ul>

                                <li><p><b>Stopped</b> means V=0 and there
                                is no known previous datapoint.  This is
                                the starting state.  If a new datapoint
                                comes while we're in the Stopped state,
                                we record the datapoint as the "old"
                                datapoint and move to the Moving state.

                                <li><p><b>Moving</b> means there is a
                                record of a recent previous datapoint
                                (Counts and Timestamp).  If we're in
                                the Moving state and no new datapoint
                                is seen for some runtime-configurable
                                amount of time (250 ms or so perhaps),
                                the algorithm forgets the old datapoint
                                and moves to the Stopped state.

                            </ul>

                        <li>

                            <p>Old datapoint: (Count, Timestamp) datapoint
                            of most recently seen encoder edge.

                            <p>Only valid when Current Motion Mode
                            is Moving.

                            <p>The count is the raw 16-bit count from
                            the firmware, extended to signed 32-bit.
                            In other words, the count in the datapoint
                            has been adjusted for rollover (both positive
                            and negative).

                            <p>The timestamp is the raw timestamp of
                            that edge.  Time has <b>not</b> been adjusted
                            for rollovers, that's handled elsewhere.

                        <li>

                            <p>R: The timestamp rollover counter.

                            <p>Only valid when Current Motion Mode
                            is Moving.

                            <p>This is the number of times the timestamp
                            clock has rolled over its 16-bit counter
                            since the "old" datapoint.

                        <li>

                            <p>Previous Time of Interest: This is a
                            relevant timestamp from the <b>previous</b>
                            time through the loop.  (Described in the
                            section on Rollover Detection below).

                            <p>Only valid when Motion Mode
                            is Moving.

                            <p>Think of this as "the time up through
                            which we've tracked TSC rollovers so far".

                    </ul>

                <li>

                    <p>On startup, the Current Motion Mode is set
                    to Stopped, so Old Datapoint, R, and PTI are all
                    dont-cares.

                <li>

                    <p>If the Motion Mode is Stopped and <b>no</b>
                    new datapoint comes in: Nothing to do, return.

                <li>
                
                    <p>If the Motion Mode is Stopped and a new datapoint comes in:

                    <ul>

                        <li>Set Old Datapoint to the new datapoint.

                        <li>Set R to 0.

                        <li>Set PTI to the new datapoint's timestamp.

                        <li>Set Motion Mode = Moving.

                        <li>Leave V at 0 for now.

                        <li>Finished!

                    </ul>

                <li>

                    <p>Motion Mode is Moving, <b>no</b> new datapoint
                    available:

                    <ul>

                        <li>TI = TSC, check for rollover.

                        <li>dT is ((TI - ODT) + (R * 2^16))

                        <li>dT > Horizon? If so V = 0 and MM = Stopped.

                        <li>Still waiting: use UBVE

                        <li>PTI = TI

                    </ul>


                <li>

                    <p>Motion Mode is Moving, new datapoint comes in:

                    <ul>

                        <li>TI = T, check for rollover

                        <li>dT is ((TI - ODT) + (R * 2^16))

                        <li>R = 0

                        <li>Use RTVE

                        <li>Old Datapoint = new datapoint.

                        <li>PTI = TI

                    </ul>


                <li><p>Rollover detection:

                    <ul>

                        <li><p>Rollover detection happens right after
                        reading the registers, before doing anything else.

                        <li><p>If there was a new datapoint, the <i>Time
                        of Interest</i> Ti is the datapoint's timestamp.

                        <li><p>If there was <b>no</b> new datapoint,
                        Ti is the TSC.

                        <li><p>If Ti &gt; 2^15, rollover detection reports
                        no rollover, records prevTi=Ti, and it's done.

                        <li><p>If Ti &gt; prevTi, rollover detection
                        reports no rollover, records prevTi=Ti, and
                        it's done.

                        <li><p>If we get here, it's a rollover!
                        Increment rollover count R.

                    </ul>

                <li>

                    <p>Relative Time Velocity Estimator (RTVE):

                    <p>Used when the C&T read indicates a new datapoint n
                    (in addition to having an old datapoint m).

                    <p>V = dS/dT = (Cn-Cm) / ((Tn-Tm) + (R*2^16))

                    <p>Reset R to 0.

                <li>

                    <p>Upper Bound Velocity Estimator (UBVE):

                    <p>Used when the C&T read indicates no new datapoint.

                    <p>So make one up (n), using the previous datapoint
                    m and the current TSC: (Cn=Cm+1, Tn=TSCn).  The Cm+1
                    is + or - 1 to be in the same direction as Vm

                    <p>V = dS/dT = (Cn-Cm) / ((Tn-Tm) + (R*2^16))

                    <p>There was no real datapoint, so we didn't use up
                    our rollovers, so we don't reset R to 0.

                <li>

                    <p>Timestamp clock speed and servo period are
                    constrained together.  C shows nearly a worst-case
                    scenario: edge comes just after the C&T read at 2,
                    gets picked up at 3, then we need to detect rollover
                    at 4.

                    <p>The servo frequency must be fast enough that Ti
                    has time to switch from an old datapoint-T to a TSC
                    before the rollover.  Two servo periods must be less
                    than half the rollover time.

            </ol>

        </td>
    </tr>
</table>





<br>

<table width=1000 cellpadding=10 border=1>

    <tr>
        <td>
            <p>Legend:
        </td>
        <td align=center valign=middle>
            <img src="legend.png">
        </td>
    </tr>

</table>




<br>

<table width=1000 cellpadding=10 border=1>

    <tr>

        <td align=center valign=middle>
            <p>
        </td>

        <td align=center valign=middle>
            <p>1. No encoder edge
        </td>

        <td align=center valign=middle>
            <p>2. Encoder edge before TSC read
        </td>

        <td align=center valign=middle>
            <p>3. Encoder edge between TSC read and C&T read
        </td>

    </tr>

    <tr>

        <td>
            <p>A. No rollover
        </td>

        <td align=center valign=middle>
            <img src="case-a1.png">
        </td>

        <td align=center valign=middle>
            <img src="case-a2.png">
        </td>

        <td align=center valign=middle>
            <img src="case-a3.png">
        </td>

    </tr>

    <tr>

        <td>
            <p>B. Rollover happens first
        </td>

        <td align=center valign=middle>
            <img src="case-b1.png">
        </td>

        <td align=center valign=middle>
            <img src="case-b2.png">
        </td>

        <td align=center valign=middle>
            <img src="case-b3.png">
        </td>

    </tr>

    <tr>

        <td>
            <p>C. Rollover happens second
        </td>

        <td align=center valign=middle>
            <img src="case-c1.png">
        </td>

        <td align=center valign=middle>
            <img src="case-c2.png">
        </td>

        <td align=center valign=middle>
            <img src="case-c3.png">
        </td>

    </tr>

    <tr>

        <td>
            <p>D. Rollover happens third
        </td>

        <td align=center valign=middle>
            <p>There is no Case D1
        </td>

        <td align=center valign=middle>
            <img src="case-d2.png">
        </td>

        <td align=center valign=middle>
            <img src="case-d3.png">
        </td>

    </tr>

</table>




<br>
<hr>
<br>

<table width=1000 cellpadding=10 border=1>
    <tr>
        <td>
            <p>Case A1: No rollover, no encoder edge.
        </td>
        <td align=center valign=middle>
            <img src="case-a1.png">
        </td>
    </tr>

    <tr>
        <td align=left valign=middle><p>MM = Stopped</td>

        <td>

            <p>Nothing to do.

        </td>

    </tr>

    <tr>

        <td><p>MM = Moving</td>

        <td>

            <p>TI is TSC.  Do rollover check.  We detect rollover here if
            PTI was pre-rollover, which means the previous time through
            the loop was case C1 or C2 or D2 or D3.

            <p>dT is ((TI - OT) + (R * 2^16)).  dT > Horizon? If so V =
            0 and MM = Stopped.

            <p>We'll wait a little longer, Use UBVE.

            <p>PTI gets the value of TI (TSC).

        </td>
    </tr>
</table>




<br>

<table width=1000 cellpadding=10 border=1>
    <tr>
        <td>
            <p>Case A2: No rollover, encoder edge before TSC read.
        </td>
        <td align=center valign=middle>
            <img src="case-a2.png">
        </td>
    </tr>

    <tr>
        <td align=left valign=middle><p>MM = Stopped</td>

        <td>

            <p>Old Datapoint = new datapoint

            <p>R = 0

            <p>MM = Moving

            <p>PTI = T

        </td>

    </tr>

    <tr>

        <td><p>MM = Moving</td>

        <td>

            <p>TI = T, do rollover detection.  We detect rollover here
            if PTI was pre-rollover.

            <p>Use RTVE.

            <p>OD = new datapoint

            <p>PTI = TI (= T)

        </td>
    </tr>
</table>




<br>

<table width=1000 cellpadding=10 border=1>
    <tr>
        <td>
            <p>Case A3: No rollover, encoder edge between TSC read and C&T read.
        </td>
        <td align=center valign=middle>
            <img src="case-a3.png">
        </td>
    </tr>

    <tr>
        <td colspan=2 align=left valign=middle>

            <p>Identical to A2.

        </td>
    </tr>
</table>




<br>

<table width=1000 cellpadding=10 border=1>
    <tr>
        <td>
            <p>Case B1: Rollover happens first, no encoder edge
        </td>
        <td align=center valign=middle>
            <img src="case-b1.png">
        </td>
    </tr>

    <tr>
        <td align=left valign=middle><p>MM = Stopped</td>

        <td align=left valign=middle>

            <p>Nothing happens.

        </td>
    </tr>

    <tr>
        <td align=left valign=middle><p>MM = Moving</td>

        <td align=left valign=middle>

            <p>TI = TSC.  Check for rollover.  Rollover detected, because
            PTI is either T or TSC from the previous servo loop, which
            ended just before the rollover.

            <p>Compute dT, check for Stop.

            <p>Use UBVE.

            <p>PTI = TI (= TSC)

        </td>
    </tr>

</table>




<br>

<table width=1000 cellpadding=10 border=1>
    <tr>
        <td>
            <p>Case B2: Rollover happens first, encoder edge before TSC read.
        </td>
        <td align=center valign=middle>
            <img src="case-b2.png">
        </td>
    </tr>

    <tr>
        <td align=left valign=middle><p>MM = Stopped</td>

        <td align=left valign=middle>

            <p>Identical to A2.

        </td>
    </tr>

    <tr>
        <td align=left valign=middle><p>MM = Moving</td>

        <td align=left valign=middle>

            <p>TI = T, check for rollover, rollover detected, because
            PTI is either T or TSC from the previous servo loop, which
            ended just before the rollover.

            <p>RTVE

            <p>PTI = TI (= T)

        </td>
    </tr>

</table>




<br>

<table width=1000 cellpadding=10 border=1>
    <tr>
        <td>
            <p>Case B3: Rollover happens first, encoder edge between TSC read and C&T read
        </td>
        <td align=center valign=middle>
            <img src="case-b3.png">
        </td>
    </tr>

    <tr>
        <td colspan=2 align=left valign=middle>

            <p>Identical to B2

        </td>
    </tr>
</table>




<br>

<table width=1000 cellpadding=10 border=1>
    <tr>
        <td>
            <p>Case C1: Rollover happens second, no encoder edge
        </td>
        <td align=center valign=middle>
            <img src="case-c1.png">
        </td>
    </tr>

    <tr>
        <td align=left valign=middle><p>MM = Stopped</td>

        <td align=left valign=middle>

            <p>Nothing to do.

        </td>
    </tr>

    <tr>
        <td align=left valign=middle><p>MM = Moving</td>

        <td align=left valign=middle>

            <p>TI = TSC, check for rollover, no rollover detected
            (because TSC is just before the rollover).

            <p>UBVE

            <p>PTI = TI (= TSC)

        </td>
    </tr>

</table>




<br>

<table width=1000 cellpadding=10 border=1>
    <tr>
        <td>
            <p>Case C2: Rollover happens second, encoder edge before TSC read.
        </td>
        <td align=center valign=middle>
            <img src="case-c2.png">
        </td>
    </tr>

    <tr>
        <td align=left valign=middle><p>MM = Stopped</td>

        <td align=left valign=middle>

            <p>Like A2.

        </td>
    </tr>

    <tr>
        <td align=left valign=middle><p>MM = Moving</td>

        <td align=left valign=middle>

            <p>TI = T, check for rollover, no rollover detected
            (because T is just before the rollover).

            <p>RTVE

            <p>PTI = TI (= T)

        </td>
    </tr>

</table>




<br>

<table width=1000 cellpadding=10 border=1>
    <tr>
        <td>
            <p>Case C3: Rollover happens second, encoder edge between TSC read and C&T read
        </td>
        <td align=center valign=middle>
            <img src="case-c3.png">
        </td>
    </tr>

    <tr>
        <td colspan=2 align=left valign=middle>

            <p>Identical to B2 and B3.

        </td>
    </tr>
</table>




<br>

<table width=1000 cellpadding=10 border=1>
    <tr>
        <td>
            <p>Case D2: Rollover happens third, encoder edge before TSC read.
        </td>
        <td align=center valign=middle>
            <img src="case-d2.png">
        </td>
    </tr>

    <tr>
        <td align=left valign=middle><p>MM = Stopped</td>

        <td align=left valign=middle>

            <p>Like A2.

        </td>
    </tr>

    <tr>
        <td align=left valign=middle><p>MM = Moving</td>

        <td align=left valign=middle>

            <p>TI = T, check for rollover, no rollover detected
            (because T is just before the rollover).

            <p>RTVE

            <p>PTI = TI (= T)

        </td>
    </tr>

</table>




<br>

<table width=1000 cellpadding=10 border=1>
    <tr>
        <td>
            <p>Case D3: Rollover happens third, encoder edge between TSC read and C&T read.
        </td>
        <td align=center valign=middle>
            <img src="case-d3.png">
        </td>
    </tr>

    <tr>
        <td colspan=2 align=left valign=middle>

            <p>Identical to D2.

        </td>
    </tr>
</table>




<br>
<hr>
<br>




</center>
</body>


</html>
